#!/usr/bin/env python
#
# *********     Gen Write Example      *********
#
#
# Available SCServo model on this example : All models using Protocol SCS
# This example is tested with a SCServo(STS/SMS/SCS), and an URT
# Be sure that SCServo(STS/SMS/SCS) properties are already set as %% ID : 1 / Baudnum : 6 (Baudrate : 1000000)
#
import os
from ftauto.scservo_sdk import *                    # Uses SCServo SDK library

class servoAction():    
    def __init__(self,id):
        self.ADDR_SCS_TORQUE_ENABLE     = 40
        self.ADDR_SCS_GOAL_ACC          = 41
        self.ADDR_SCS_GOAL_POSITION     = 42
        self.ADDR_SCS_GOAL_SPEED        = 46
        self.ADDR_SCS_PRESENT_POSITION  = 56        
           # Default setting
        self.SCS_ID                 = int(id)                 # SCServo ID : 1
        self.BAUDRATE                    = 1000000           # SCServo default baudrate : 1000000
        self.DEVICENAME                  = '/dev/ttyUSB0'    # Check which port is being used on your controller
        self.protocol_end                = 0                  # SCServo bit end(STS/SMS=0, SCS=1)       
        # Initialize PortHandler instance
        # Set the port path
        # Get methods and members of PortHandlerLinux or PortHandlerWindows
        # Open port   

    def forwardUp(self,fvel=None,facc=None,fdis=None,flag=None):
        if int(flag)>0:        
            SCS_MINIMUM_POSITION_VALUE  = 0       # SCServo will rotate between this value
            SCS_MAXIMUM_POSITION_VALUE  = 4059   
        else:
            SCS_MINIMUM_POSITION_VALUE  = 4059       # SCServo will rotate between this value
            SCS_MAXIMUM_POSITION_VALUE  = 0   

        self.SCS_MOVING_STATUS_THRESHOLD = 20          # SCServo moving status threshold
        self.scs_goal_position = [SCS_MINIMUM_POSITION_VALUE, SCS_MAXIMUM_POSITION_VALUE]         # Goal position                                                # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"
        
        # Control table address    
        index = 0  
        SCS_MOVING_SPEED            = int(fvel)           # SCServo moving speed
        SCS_MOVING_ACC              = int(facc)           # SCServo moving acc        
        scc_time_spend = abs(int(int(fdis)/int(fvel)))          
        self.portHandler = PortHandler(self.DEVICENAME)

        # Initialize PacketHandler instance
        # Get methods and members of Protocol
        self.packetHandler = PacketHandler(self.protocol_end)
        if self.portHandler.openPort():
            print("Succeeded to open the port")
        else:
            print("Failed to open the port")

        # Set port baudrate
        if self.portHandler.setBaudRate(self.BAUDRATE):
            print("Succeeded to change the baudrate")
        else:
            print("Failed to change the baudrate")              
        # Write SCServo acc
        scs_comm_result, scs_error = self.packetHandler.write1ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_GOAL_ACC, SCS_MOVING_ACC)
        if scs_comm_result != COMM_SUCCESS:
            print("%s" % self.packetHandler.getTxRxResult(scs_comm_result))
        elif scs_error != 0:
            print("%s" % self.packetHandler.getRxPacketError(scs_error))

        # Write SCServo speed
        scs_comm_result, scs_error = self.packetHandler.write2ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_GOAL_SPEED, SCS_MOVING_SPEED)
        if scs_comm_result != COMM_SUCCESS:
            print("%s" % self.packetHandler.getTxRxResult(scs_comm_result))
        elif scs_error != 0:
            print("%s" % self.packetHandler.getRxPacketError(scs_error))
        starttime = time.time()
        isStop = False
        while 1:
            # Write SCServo goal position
            scs_comm_result, scs_error = self.packetHandler.write2ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_GOAL_POSITION, self.scs_goal_position[index])
            if scs_comm_result != COMM_SUCCESS:
                print("%s" % self.packetHandler.getTxRxResult(scs_comm_result))
            elif scs_error != 0:
                print("%s" % self.packetHandler.getRxPacketError(scs_error))
            
            while 1:
                # Read SCServo present position
                scs_present_position_speed, scs_comm_result, scs_error =self.packetHandler.read4ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_PRESENT_POSITION)
                if scs_comm_result != COMM_SUCCESS:
                    print(self.packetHandler.getTxRxResult(scs_comm_result))
                elif scs_error != 0:
                    print(self.packetHandler.getRxPacketError(scs_error))

                scs_present_position = SCS_LOWORD(scs_present_position_speed)
                scs_present_speed = SCS_HIWORD(scs_present_position_speed)
                print("[ID:%03d] GoalPos:%03d PresPos:%03d PresSpd:%03d" 
                      % (self.SCS_ID, self.scs_goal_position[index], scs_present_position, SCS_TOHOST(scs_present_speed, 15)))
                print(time.time()-starttime)
                if time.time()-starttime>scc_time_spend:
                    isStop = True
                    break
                if not (abs(self.scs_goal_position[index] - scs_present_position_speed) > self.SCS_MOVING_STATUS_THRESHOLD):
                    isStop = True
                
            # Change goal position
            if isStop:break 
            if index == 0:
                index = 1
            else:
                index = 0               
    
    def getLoc(self):
        self.portHandler = PortHandler(self.DEVICENAME)
        # Initialize PacketHandler instance
        # Get methods and members of Protocol
        self.packetHandler = PacketHandler(self.protocol_end)
        if self.portHandler.openPort():
            print("Succeeded to open the port")
        else:
            print("Failed to open the port")
        # Set port baudrate
        if self.portHandler.setBaudRate(self.BAUDRATE):
            print("Succeeded to change the baudrate")
        else:
            print("Failed to change the baudrate")           
        scs_present_position_speed, scs_comm_result, scs_error =self.packetHandler.read4ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_PRESENT_POSITION)               
        scs_present_position = SCS_LOWORD(scs_present_position_speed)        
        return scs_present_position        
         
    def stop(self):
        # Close port
        # Control table address           
        self.portHandler = PortHandler(self.DEVICENAME)

        # Initialize PacketHandler instance
        # Get methods and members of Protocol
        self.packetHandler = PacketHandler(self.protocol_end)
        if self.portHandler.openPort():
            print("Succeeded to open the port")
        else:
            print("Failed to open the port")

        # Set port baudrate
        if self.portHandler.setBaudRate(self.BAUDRATE):
            print("Succeeded to change the baudrate")
        else:
            print("Failed to change the baudrate")              
        scs_comm_result, scs_error = self.packetHandler.write1ByteTxRx(self.portHandler, self.SCS_ID, self.ADDR_SCS_TORQUE_ENABLE, 0)
        if scs_comm_result != COMM_SUCCESS:
            print("%s" % self.packetHandler.getTxRxResult(scs_comm_result))
        elif scs_error != 0:
            print("%s" % self.packetHandler.getRxPacketError(scs_error))       
        self.portHandler.closePort()


